%作业3-23

%%
%注意数据存储方式，是组号(关节1/2)- 时间 - 数据类型（加速度速度或者位置）
torque_info = zeros(2,length(1:simu_step),3);
joint_all_info = zeros(2,length(1:simu_step),3);
abb = zeros(length(1:simu_step),2);


%%

for i = 1:simu_step
    x = pos_s_traj_value(i,1);%这个是笛卡尔空间的位置
    joint_pos= -inverseKinematics(x, y, a_dj.l,a_dj.l);%得到关节空间位置
    a_dj.joint_state = joint_pos;
    jacobi_m = a_dj.get_jacobi(0);
    joint_vel = jacobi_m \ [pos_s_traj_value(i,2);dy];
    a_dj.joint_state(:,2) = joint_vel;
    d_jacobi_m = a_dj.get_dot_jacobi(0);
    joint_acc = jacobi_m \ [pos_s_traj_value(i,3);dy] ...
        - jacobi_m\d_jacobi_m/jacobi_m * joint_vel;
    a_dj.joint_state(:,3) = joint_acc;
    joint_info = [joint_pos,joint_vel,joint_acc];
    joint_all_info(:,i,:) = joint_info;
    torque_info(:,i,:) = a_dj.inverse_dynamic(0,joint_info,joint_info.*a_dj.n_factor).*a_dj.n_factor;
    
    %这里是对角速度积分，验证位置是否合理
    if(i ~= 1)
        abb(i,:) = abb(i-1,:)+joint_vel'*simu_period;
    else
        abb(i,:) = joint_pos';
    end
end

%%
figure;
plot(0:simu_period:(simu_step-1)*simu_period,torque_info(1,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,torque_info(2,:,1),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
legend('joint1','joint2');
title("总力矩");
figure;
plot(0:simu_period:(simu_step-1)*simu_period,torque_info(1,:,2),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,torque_info(2,:,2),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
legend('joint1','joint2');
title("线性");
figure;
plot(0:simu_period:(simu_step-1)*simu_period,torque_info(1,:,3),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,torque_info(2,:,3),'m-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
hold on
legend('joint1','joint2');
title("非线性");


%% 验证一下逆解的关节速度能不能完成关节空间是否对应，之所以关节速度不是平的，因为
%解得的关节位置不是线性的，合理
figure;
yyaxis left
plot(0:simu_period:(simu_step-1)*simu_period,joint_all_info(1,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,abb(:,1),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
hold on
yyaxis right
plot(0:simu_period:(simu_step-1)*simu_period,joint_all_info(1,:,2),'-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
legend('inv res','int res by omega','omega');
title("pos S traj follow joint1");


figure;
yyaxis left
plot(0:simu_period:(simu_step-1)*simu_period,joint_all_info(2,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,abb(:,2),'r--^','MarkerIndices',1:12000:simu_step,'MarkerSize',2);
hold on
yyaxis right
plot(0:simu_period:(simu_step-1)*simu_period,joint_all_info(2,:,2),'-o','MarkerIndices',1:5000:simu_step,'MarkerSize',2);
legend('inv res','int res by omega','omega');
title("pos S traj follow joint2");
